/*
  robotLac.h - Library to control robotLac and sensors.
  Created by Leandro F. Lichtensztein, April 24, 2013.
  Released into the public domain.
*/

#ifndef robotLac_h
#define robotLac_h
#include "Arduino.h"
#include "..\Servo\Servo.h"
#include "..\Wire\Wire.h"


class robotLac{

    public:
        robotLac(int pin, int robot);
        void begin();
        void detener();
        void retroceder();
        void avanzar();
        void horario();
        void antihorario();
        int getEstado();
        void setVelocidadD(int velocidad);
        void setVelocidadI(int velocidad);
        void setVelocidadG(int velocidad);
        int getVelocidadD();
        int getVelocidadI();
        int getVelocidadG();
        void derecha();
        void izquierda();


    private:
        Servo myservoV1, myservoV2, myservoV3, myservoV4; //Ruedas
        Servo myservoA1, myservoA2, myservoA3, myservoA4; //Angulo
        int estado, on_off_pin, velD, velI, velG;
        int V1, V2, V3, V4, A1R, A2R, A3R, A4R, A1D, A2D, A3D, A4D;
};

class Infrared_Sensor{
    public:
        Infrared_Sensor(int analogPin);
        int read();

    private:
        int pin;
};

class Distance_Sensor{
    public:
        Distance_Sensor(int triggerPin, int echoPin);
        void begin();
        float get_distance();

    private:
        int trigger;
        int echo;
        float distance;
};

class Color_Sensor{
    public:
        Color_Sensor(int s0Pin, int s1Pin, int s2Pin, int s3Pin, int outPin);
        void begin();
        void calc_RGB();
        int get_R();
        int get_G();
        int get_B();
        char get_Color();

    private:
        int s0,s1,s2,s3,out;
        int red, green, blue;
        int R, G, B;
        void convertRGB();
        void readSensor();
};

class Gyroscope_Sensor{
    public:
        Gyroscope_Sensor();
        void begin();
        double get_Z_angle();

    private:
        int L3G4200D_Address;
        double x, y, z;
        double Z_ang, time, old_time, scale;
        void setupL3G4200D(int scale);
        void getGyroValues();
        void writeRegister(int deviceAddress, byte address, byte val);
        int readRegister(int deviceAddress, byte address);
};

#endif
